// footballrobot.pde - Footballrobot for cellphone control
// (c) Kimmo Karvinen & Tero Karvinen http://BotBook.com
#include <Servo.h>
#include <SoftwareSerial.h>
// Keep track of how far along we are reading a message
const int READY = 1; // Ready to receive a message
const int RECEIVED_START = 2; // Received the start character: 'S'
const int RECEIVED_STEERING = 3; // Received the left speed
 



int bluetoothTx = 2;
int bluetoothRx = 3;
 
int state = READY;
// Define the pins and declare the servo objects.
int servoSteeringPin=9; 
Servo servoSteering;
// Define initial angle of servo steering motor
int steeringAngle = 90 ;
SoftwareSerial bluetooth(bluetoothTx, bluetoothRx);
// Temporary speed variables used while we are processing a command.
int newSteeringAngle;
 
int ledPin=13; // LED output pin


void setup()
{
	pinMode(ledPin, OUTPUT);
	digitalWrite(ledPin, HIGH);
	servoSteering.attach(servoSteeringPin);
        delay(100);
        bluetooth.println("U,9600,N");
        bluetooth.begin(9600);    
        delay(100);        
        Serial.begin(9600);
        while (!Serial) {
            ; // wait for serial port to connect. Needed for Leonardo only
        }
        
	stopMoving();		
	digitalWrite(ledPin, LOW);
        delay(100);
        Serial.println("PASSO 0 - START");
}
void loop()
{
  execute();
//   servoSteering.write(90); 
}


void execute(){
     if (bluetooth.available()) {
          Serial.println("A");
	  int ch = bluetooth.read(); // Read a character
	  switch (state) {
	  case READY:
				if ('S' == ch) {
                                                 Serial.println("PASSO 1 - READY");
						state = RECEIVED_START; // We'll be in this state
						// next time through loop()
				}
				else if ('?' == ch) {
                                          Serial.println("PASSO 0 - SONO IN ASCOLTO");
					  bluetooth.print("L"); // Let the phone know we are listening
				}
				break;
		case RECEIVED_START:
				if (ch >= 0 && ch <= 9) { // Make sure the values are in range
						state = RECEIVED_STEERING;
                                                Serial.println("PASSO 2 STERZATA");
                                                Serial.println(ch);
						// Set the temporary Steering value
						newSteeringAngle = map(int(ch), 0, 9, 0, 179);
				}
				else { // Invalid input--go back to the ready state
						state = READY;
                                                Serial.println("ERRORE");
                                                 Serial.println(ch);
                                                
				}
				break;
		 
		case RECEIVED_STEERING:
				if ('U' == ch) { // Reached the end of the message
					steeringAngle = newSteeringAngle; // Set the speeds
					 Serial.println("F");
			
                                }
				state = READY;
				
				break;
        }
    }
    if (state == READY) {
		 move();
     }
    //delay(10); // Give the microcontroller a brief rest
}
void move()
{
	servoSteering.write(steeringAngle);
	 
}
void stopMoving()
{
	steeringAngle = 90;
}	
